/*
 *  camwireFunctions.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2008-01-23.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "camwireFunctions.h"

#ifdef CAMWIRE
typedef struct 
{
    int shadowlevel;
    enum {stopped, running} activity;
    enum {continuous, single} acqtype;
    enum {internal, external} trigger;
    enum {falling, rising} polarity;
    double shutter;
    double framerate;
    int width;
    int height;
    Camwire_pixel coding;
    int left;
    int top;
    double white_bal_blue;
    double white_bal_red;
    int num_buffers;
    long int framenumber;
}
Settings_t;

// Private prototype declarations
static void cleanup(const Camwire_handle c_handle);
static void wait_frametime(const Camwire_handle c_handle, const double multiple);
static void errorexit(const Camwire_handle c_handle, const int cam, const char *msg);
static int get_user_cam(const Camwire_handle *handle_array, const int num_cameras);
static int get_named_cam(const Camwire_handle *handle_array, const int num_cameras, const char *chip_id_string);
static void get_camera_settings(const Camwire_handle c_handle, Settings_t *set);
static void settings_load(const Camwire_handle c_handle, Settings_t *set, const char *settings_filename);
static int update_settings(FILE *settingsfile, Settings_t *new_settings);
static void update_camera(const Camwire_handle c_handle, Settings_t *new_settings, const Settings_t *set);
static void check(const int scanresult, const char *tag, int *gotinput);
static char *skip_whitespace(const char *string);
static char *skip_non_whitespace(const char *string);

#define MAX_FILENAME		1000
#define SAFE_FRAME_PERIOD	1.5
#define MAX_SETTINGS_LINE	500 //Max number of settings in config file
#define SETTINGS_COMMENT	'#' //Character indicating commented line in config file
#define WHITESPACE		" \t\r\n"
#define BUF_LOW_MARK		0.1
#define BUF_HIGH_MARK		0.9

Settings_t settings;
static const char *settings_format =
        "shadowlevel     %d\n"
	"activity        %d\n"
	"acqtype         %d\n"
	"trigger         %d\n"
	"polarity        %d\n"
	"shutter         %lg\n"
	"framerate       %lg\n"
	"width           %d\n"
	"height          %d\n"
	"coding          %d\n"
	"left            %d\n"
	"top             %d\n"
        "white_bal_blue  %lg\n"
        "white_bal_red   %lg\n"
	"num_buffers     %d\n"
	"framenumber     %ld\n"
	"save_num_images %d\n"
	"save_delay      %lg\n"
	"imagefilename   %s\n";
/* This is dreadful design, because the number and order of the fields
   in the format string above are important.  There must be exactly one
   conversion for each member of the Settings_t structure above.  It is
   used in settings_save() and update_settings(). */


Camwire_handle cwCaptureFromDCAM()
{
    //Variable declarations
    struct timespec;
    char chip_id_string[CAMWIRE_ID_MAX_CHARS+1] = "\0";
    char settings_filename[]="dcam_settings.txt";
    Camwire_handle *handle_array = NULL;
    Camwire_handle c_handle = NULL;
    int num_cameras, current_cam;
    int retry, bad_bus;
    int runsts;
    void *capturebuffer = NULL;
//    IplImage *camFrame;

    /*
    int more_options;
    int option;
    int display_return;
    int key;
    fd_set rfds;
    struct timeval tv;
    */
    
    //=============================
    // Initialize the camera bus
    //=============================
    handle_array = camwire_bus_create(&num_cameras);
    if (num_cameras < 0 || handle_array == NULL)
    { 	/* Could be the known kernel 1394 cycle master bug. */
	printf("Error initializing the bus: trying reset");
	retry = 1;
	bad_bus = 1;
	while (retry <= 5 && bad_bus)
	{
	    printf(" %d", retry);
	    fflush(stdout);
	    camwire_bus_reset();
	    sleep(1);
	    handle_array = camwire_bus_create(&num_cameras);
	    if (num_cameras >= 0 && handle_array != NULL)  bad_bus = 0;
	    retry++;
	}
	if (bad_bus)
	{
	    errorexit(NULL, -1,
		      "Could not initialize the camera bus.  "
		      "There may not be a camera connected.");
	}
	else
	{
	    printf("\n\n");
	    fflush(stdout);
	}
    }
    if (num_cameras == 0)
    {
	errorexit(NULL, -1, "Could not find a camera.");
    }
    if (num_cameras == 1)  printf("Found one camera.\n");
    else                   printf("Found %d cameras.\n\n", num_cameras);
    fflush(stdout);

    //========================
    // 
    //========================
    
    /* Identify the camera to use: */
    if (chip_id_string[0] != '\0')
    {
	/* Find the camera named on the command line: */
	current_cam = get_named_cam(handle_array, num_cameras, chip_id_string);
    }
    else
    {
	/* Ask user which camera to use:*/
	current_cam = get_user_cam(handle_array, num_cameras);
    }
    
    //========================
    // Initialize the camera and our local copy of its settings. Then apply user settings.
    //========================
    
    c_handle = handle_array[current_cam];
    if (camwire_create(c_handle) != CAMWIRE_SUCCESS)
    {
	errorexit(NULL, current_cam, "Could not initialize camera.");
    }
    get_camera_settings(c_handle, &settings);
    settings_load(c_handle, &settings, settings_filename); 
    return c_handle;
}

void cwCapture( )
{
 
    return;
}

void cwQueryFrame( Camwire_handle c_handle )
{
 
    return;
}


/*
  ----------------------------------------------------------------------
  Returns the camera to work with, which is the camera number with
  matching chip identifier string.  Displays the camera's identifier
  data and creates it as well.
*/
static int get_named_cam(const Camwire_handle *handle_array,
			 const int num_cameras,
			 const char *chip_id_string)
{
    int c, current_cam;
    Camwire_id camid;

    current_cam = -1;;
    for (c = 0; c < num_cameras; c++)
    {
	if (camwire_get_identifier(handle_array[c], &camid) !=
	    CAMWIRE_SUCCESS)
	{
	    errorexit(NULL, c, "Could not get the identifier.");
	}
	if (strncmp(camid.chip, chip_id_string, CAMWIRE_ID_MAX_CHARS) ==
	    0)
	{
	    current_cam = c;
	    break;
	}
    }

    if (current_cam < 0)
    {
	printf("Could not find camera with ID %s.\n", chip_id_string);
	fflush(stdout);
	errorexit(NULL, current_cam,
		  "Try running without specifying an ID.");
    }

    printf("Vendor name:       %s\n", camid.vendor);
    printf("Model name:        %s\n", camid.model);
    printf("Vendor & chip ID:  %s\n", camid.chip);
    fflush(stdout);
    return(current_cam);
}


/*
  ----------------------------------------------------------------------
  Returns the camera to work with, which is 0 if there is only one
  camera or the camera number chosen interactively by the user if there
  is more than one camera.  Displays the camera's identifier data and
  creates it as well.
*/
static int get_user_cam(const Camwire_handle *handle_array,
			const int num_cameras)
{
    int c, current_cam;
    int got_one;
    Camwire_id camid;

    for (c = 0; c < num_cameras; c++)
    {
	if (camwire_get_identifier(handle_array[c], &camid) !=
	    CAMWIRE_SUCCESS)
	{
	    errorexit(NULL, c, "Could not get the identifier.");
	}
	if (num_cameras > 1)  printf("\nCamera %d:\n", c + 1);
	printf("Vendor name:       %s\n", camid.vendor);
	printf("Model name:        %s\n", camid.model);
	printf("Vendor & chip ID:  %s\n", camid.chip);
	fflush(stdout);
    }

    if (num_cameras == 1)
    {
	current_cam = 0;
    }
    else
    {
	got_one = 0;
	while (!got_one)
	{
	    current_cam = -1;
	    while (current_cam < 0 || current_cam > num_cameras)
	    {
		printf("\nSelect a camera number "
		       "(from 1 to %d, 0 quits): ", num_cameras);
		fflush(stdout);
		scanf("%d", &current_cam);
//		clear_stdin();
	    }
	    current_cam--;
	    if (current_cam < 0) 
	    {
		camwire_bus_destroy();
		putchar('\n');
		fflush(stdout);
		exit(EXIT_SUCCESS); 	/* Normal exit.*/
	    }
	    else
	    {
		got_one = 1;
	    }
	}
    }
    return(current_cam);
}


/*
  ----------------------------------------------------------------------
  Reads the camera's parameters and default values into the settings
  structure.
*/
static void get_camera_settings(const Camwire_handle c_handle,
			       Settings_t *set)
{
    int settingsval;
    int camwire_result;

    camwire_get_stateshadow(c_handle, &set->shadowlevel);

    camwire_get_run_stop(c_handle, &settingsval);
//    if (settingsval)  set_new->activity = running;
//    else              set->activity = stopped;

    camwire_get_single_shot(c_handle, &settingsval);
//    if (settingsval)  set->acqtype = single;
//    else              set->acqtype = continuous;

    camwire_result = camwire_get_white_balance(c_handle, &set->white_bal_blue,
            &set->white_bal_red);
    if (camwire_result != CAMWIRE_SUCCESS) {
        fprintf(stderr, "Could not get camera white balance: Camera may "
                "not support it.\n");
        set->white_bal_blue = set->white_bal_red = 0;
    }

    camwire_result = camwire_get_trigger_source(c_handle, &settingsval);
    if (camwire_result != CAMWIRE_SUCCESS)
    {
	fprintf(stderr, "Could not get camera trigger source: Camera may "
		"not support it.\n");
	settingsval = 0;
    }
//    if (settingsval)  set->trigger = external;
//    else              set->trigger = internal;

    camwire_result = camwire_get_trigger_polarity(c_handle, &settingsval);
    if (camwire_result != CAMWIRE_SUCCESS)
    {
	fprintf(stderr, "Could not get camera trigger polarity: Camera may "
		"not support it.\n");
	settingsval = 0;
    }
//    if (settingsval)  set->polarity = rising;
//    else              set->polarity = falling;

    camwire_result = camwire_get_shutter(c_handle, &set->shutter);
    if (camwire_result != CAMWIRE_SUCCESS)
    {
	fprintf(stderr, "Could not get camera shutter: Camera may "
		"not support it.\n");
	set->shutter = 0;
    }

    camwire_get_framerate(c_handle, &set->framerate);

    camwire_get_frame_size(c_handle, &set->width, &set->height);

    camwire_get_pixel_coding(c_handle, &set->coding);

    camwire_get_frame_offset(c_handle, &set->left, &set->top);

    camwire_get_num_framebuffers(c_handle, &set->num_buffers);

    camwire_get_framenumber(c_handle, &set->framenumber);
}


/*
  ----------------------------------------------------------------------
  Loads the current monitor settings from file and updates the
  camera.
*/
static void settings_load(const Camwire_handle c_handle,
			  Settings_t *set,
			  const char *settings_filename)
{
    FILE *settingsfile;
    Settings_t new_settings;
    int gotinput;

    settingsfile = fopen(settings_filename, "r");
    if (settingsfile != NULL)
    {
	/* Read any new settings with old settings as default: */
	new_settings = *set;
	gotinput = update_settings(settingsfile, &new_settings);
	fclose(settingsfile);
	if (gotinput)
	{
	    printf("Settings loaded from \"%s\".\n", settings_filename);
	    
	    /* Write any changes to camera and read back actual: */
	    update_camera(c_handle, &new_settings, set);
	    get_camera_settings(c_handle, set);
	    
	}
	else	
	{
	    fprintf(stderr, "No settings read from %s.\n",
		    settings_filename);
	}
    }
    else
    {
	fprintf(stderr, "Could not open the settings file \"%s\" "
		"for reading.\n", settings_filename);
    }
}


/*
  ----------------------------------------------------------------------
  Reads the opened settings file, changing only the fields present in
  the file.  The new settings are assumed already to be initialized with
  old settings.
*/
static int update_settings(FILE *settingsfile, Settings_t *new_settings)
{
    int gotinput, empty, comment;
    char linebuffer[MAX_SETTINGS_LINE+1];
    char *lineptr;
    char *new_tag_str, *value_str;
    char *ref_tag_str, *format_str;

    gotinput = 0;
    while(!feof(settingsfile))
    {
	/* Read lines until not empty and not a comment: */
	empty = 1;
	comment = 1;
	while (empty || comment)
	{
	    lineptr =
		fgets(linebuffer, MAX_SETTINGS_LINE, settingsfile);
	    if (lineptr == NULL)  break;
	    empty = (strlen(skip_whitespace(linebuffer)) == 0);
	    comment = (linebuffer[0] == SETTINGS_COMMENT);
	}
	if (lineptr == NULL)  break;

	/* Separate input tag string and value string: */
	new_tag_str = skip_whitespace(linebuffer);
	lineptr = skip_non_whitespace(new_tag_str);
	*lineptr = '\0';
	lineptr++;
	value_str = skip_whitespace(lineptr);
	if (strlen(value_str) >= 2)  /* Including a newline char.*/
	{
	    /* Find corresponding tag and conversion in the format: */
	    ref_tag_str = strstr(settings_format, new_tag_str);
	    if (ref_tag_str != NULL)
	    {
		lineptr = skip_non_whitespace(ref_tag_str);
		format_str = skip_whitespace(lineptr);
		
		if (strcmp(new_tag_str, "shadowlevel") == 0)
		{
		    check(sscanf(value_str, format_str,
				 &new_settings->shadowlevel),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "activity") == 0)
		{
		    check(sscanf(value_str, format_str, &new_settings->activity),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "acqtype") == 0)
		{
		    check(sscanf(value_str, format_str, &new_settings->acqtype),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "trigger") == 0)
		{
		    check(sscanf(value_str, format_str, &new_settings->trigger),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "polarity") == 0)
		{
		    check(sscanf(value_str, format_str, &new_settings->polarity),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "shutter") == 0)
		{
		    check(sscanf(value_str, format_str, &new_settings->shutter),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "framerate") == 0)
		{
		    check(sscanf(value_str, format_str, &new_settings->framerate),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "width") == 0)
		{
		    check(sscanf(value_str, format_str, &new_settings->width),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "height") == 0)
		{
		    check(sscanf(value_str, format_str, &new_settings->height),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "coding") == 0)
		{
		    check(sscanf(value_str, format_str, (int *)
				 &new_settings->coding), new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "left") == 0)
		{
		    check(sscanf(value_str, format_str, &new_settings->left),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "top") == 0)
		{
		    check(sscanf(value_str, format_str, &new_settings->top),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "white_bal_blue") == 0)
		{
		    check(sscanf(value_str, format_str,
				 &new_settings->white_bal_blue),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "white_bal_red") == 0)
		{
		    check(sscanf(value_str, format_str,
				 &new_settings->white_bal_red),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "num_buffers") == 0)
		{
		    check(sscanf(value_str, format_str,
				 &new_settings->num_buffers),
			  new_tag_str, &gotinput);
		}
		else if (strcmp(new_tag_str, "framenumber") == 0)
		{
		    check(sscanf(value_str, format_str,
				 &new_settings->framenumber),
			  new_tag_str, &gotinput);
		}
                else
		{
		    fprintf(stderr, "Cammonitor internal error:  Don't "
			    "know how to read tag `%s' in the settings "
			    "format string.\n", new_tag_str);
		}
	    }
	    else
	    {
		fprintf(stderr,
			"Unrecognized tag `%s' in settings file.\n",
			new_tag_str);
	    }
	}
	else
	{
	    fprintf(stderr, "No value for tag `%s' in settings file.\n",
		    new_tag_str);
	}
    }  /* while */

    return(gotinput);
}


/*
  ----------------------------------------------------------------------
  Update the camera with new settings.  Note that the order of register
  writes may be significant for some cameras after power-up or
  reset/initilize, hence the elaborate cascade.
*/
static void update_camera(const Camwire_handle c_handle,
			  Settings_t *new_settings,
			  const Settings_t *set)
{
    int dotherest;
	
    dotherest = 0;
    if (dotherest ||
	new_settings->num_buffers != set->num_buffers)
    {
	camwire_set_num_framebuffers(c_handle, new_settings->num_buffers);
	dotherest = 1;
    }
    if (dotherest ||
	new_settings->left != set->left || new_settings->top != set->top)
    {
	camwire_set_frame_offset(c_handle, new_settings->left, new_settings->top);
	dotherest = 1;
    }
    if (dotherest ||
	new_settings->width != set->width || new_settings->height != set->height)
    {
	camwire_set_frame_size(c_handle, new_settings->width, new_settings->height);
	dotherest = 1;
    }
    /* Do offsets a second time in case they were not allowed by frame
       size the first time: */
    if (dotherest ||
	new_settings->left != set->left || new_settings->top != set->top)
    {
	camwire_set_frame_offset(c_handle, new_settings->left, new_settings->top);
	dotherest = 1;
    }
    if (dotherest ||
	new_settings->coding != set->coding)
    {
	camwire_set_pixel_coding(c_handle, new_settings->coding);
	dotherest = 1;
    }
    if (dotherest ||
	fabs(new_settings->framerate - set->framerate) > 1e-6)
    {
	camwire_set_framerate(c_handle, new_settings->framerate);
	dotherest = 1;
    }
    if (dotherest ||
	new_settings->trigger != set->trigger)
    {
	camwire_set_trigger_source(c_handle,
				   new_settings->trigger);
	dotherest = 1;
    }
    if (dotherest ||
	new_settings->polarity != set->polarity)
    {
	camwire_set_trigger_polarity(c_handle,
				     new_settings->polarity);
	dotherest = 1;
    }
    if (dotherest ||
	new_settings->shutter != set->shutter)
    {
	camwire_set_shutter(c_handle, new_settings->shutter);
	dotherest = 1;
    }
    if ((dotherest ||
	new_settings->white_bal_blue != set->white_bal_blue ||
	new_settings->white_bal_red != set->white_bal_red) ) 
    {
	camwire_set_white_balance(c_handle, new_settings->white_bal_blue,
				  new_settings->white_bal_red);
	dotherest = 1;
    }
    if (dotherest ||
	new_settings->acqtype != set->acqtype)
    {
	camwire_set_single_shot(c_handle, new_settings->acqtype);
	dotherest = 1;
    }
    if (dotherest ||
	new_settings->activity != set->activity)
    {
	camwire_set_run_stop(c_handle, new_settings->activity);
	dotherest = 1;
    }
    if (dotherest ||
	new_settings->shadowlevel != set->shadowlevel)
    {
	camwire_set_stateshadow(c_handle, new_settings->shadowlevel);
	dotherest = 1;
    }
}


/*
  ----------------------------------------------------------------------
  Sets gotinput if scanresult is 1, else prints error message.
*/
static void check(const int scanresult, const char *tag, int *gotinput)
{
    if (scanresult == 1)
    {
	*gotinput = 1;
    }
    else
    {
	fprintf(stderr,
		"Error reading value of `%s' in settings file.\n", tag);
    }
}

/*
  ----------------------------------------------------------------------
  Return a pointer to the 1st non-whitespace character in null-delimited
  string.  If the string contains only whitespace, returns a pointer to
  an empty string (pointer to the null character at the end of the
  string).
*/
static char * skip_whitespace(const char *string)
{
    return((char *) &string[strspn(string, WHITESPACE)]);
}

/*
  ----------------------------------------------------------------------
  Return a pointer to the 1st whitespace character in null-delimited
  string.  If the string contains no whitespace, returns a pointer to an
  empty string (pointer to the null character at the end of the string).
*/
static char * skip_non_whitespace(const char *string)
{
    return((char *) &string[strcspn(string, WHITESPACE)]);
}


/*
  ----------------------------------------------------------------------
  Checks the number of pending filled buffers and flushes some of them
  if there are too many.  This helps to ensure that frame numbers are
  accurately updated when frames are dropped.  Otherwise buffer
  overflows may result in the user not knowing if frames have been lost.
*/
void manage_buffer_level(const Camwire_handle c_handle,
				FILE *logfile)
{
    int total_frames, current_level, num_to_flush;

    camwire_get_num_framebuffers(c_handle, &total_frames);
    if (total_frames < 3)  return;
    camwire_get_framebuffer_lag(c_handle, &current_level);
    current_level++; 	/* Buffer lag does not count current frame.*/

    /* It seems that the DMA buffers sometimes do not fill up
       completely, hence the extra -1 in the if expression below: */
    if (current_level >= total_frames - 1)
    { 	/* Hit the ceiling.*/
	num_to_flush = total_frames;
	camwire_flush_framebuffers(c_handle, num_to_flush, NULL, NULL);
	if (logfile != NULL)
	{
	    fprintf(logfile, "Frame buffers overflowed.  "
		    "Frame numbers may no longer be in synch.\n");
	    fflush(logfile);
	}
    }
    else if (current_level + 0.5 >= BUF_HIGH_MARK*total_frames)
    {
	num_to_flush = current_level - (int)BUF_LOW_MARK*total_frames;
	camwire_flush_framebuffers(c_handle, num_to_flush, NULL, NULL);
    }
    /* else don't flush. */
}


/*
  ----------------------------------------------------------------------
  Sleeps for multiple times one frame period, where multiple is a double.
*/
static void wait_frametime(const Camwire_handle c_handle,
			   const double multiple)
{
    double framerate, frameperiod;
    struct timespec nap;
    
    camwire_get_framerate(c_handle, &framerate);
    frameperiod = multiple/framerate;
    nap.tv_sec = (__time_t) frameperiod;
    nap.tv_nsec = (long int) ((frameperiod - nap.tv_sec)*1e9);
    nanosleep(&nap, NULL);
}

/*
  ----------------------------------------------------------------------
  Prints the given error message (including the camera number if it is
  non-negative) to stderr, cleans up, and exits with a return status of
  1.
*/
static void errorexit(const Camwire_handle c_handle, const int cam,
		      const char *msg)
{
    fflush(stdout);  /* libdc1394 writes error messages to stdout.*/
    if (cam < 0)  fprintf(stderr, "\n%s\n", msg);
    else          fprintf(stderr, "\nCamera %d: %s\n", cam + 1, msg);
    cleanup(c_handle);
    exit(EXIT_FAILURE);
}

/*
  ----------------------------------------------------------------------
  Resets the console input to default settings, stops the camera, closes
  the display window, frees the frame buffer, and destroys the camera
  object and its bus.  Should be safe to call with null arguments.
*/
static void cleanup(const Camwire_handle c_handle)
{
    if (c_handle != NULL)
    {
	camwire_set_run_stop(c_handle, 0);
	wait_frametime(c_handle, SAFE_FRAME_PERIOD);
    }
//    if (display_initialized)  display_destroy(d_handle);
//    display_initialized = 0;
    camwire_destroy(c_handle);
    camwire_bus_destroy();
    putchar('\n');
    fflush(stdout);
}
#endif
